/*
 * R/C receiver input monitoring for Flying Fox
 *
 * by Zik Saleeba 2008-03-05
 */

#include "ffox.h"
#include "AT91SAM7X256.h"
#include "lib_AT91SAM7X256.h"
#include "servo.h"
#include "timer.h"
#include "receiver.h"

/* module variables */
static struct RawTime ReceiverCompletedStartPos[NUM_RECEIVER_INPUTS];
static struct RawTime ReceiverCompletedEndPos[NUM_RECEIVER_INPUTS];
static struct RawTime ReceiverStartPos[NUM_RECEIVER_INPUTS];

static unsigned int ReceiverPinMask[NUM_RECEIVER_INPUTS] = 
{ 
    RECEIVER1_INPUT, 
    RECEIVER2_INPUT, 
    RECEIVER3_INPUT, 
    RECEIVER4_INPUT, 
    RECEIVER5_INPUT
};    

/* prototypes */
static void Receiver_IsrHandler(void) __attribute__ ((interrupt("IRQ")));


/*
 * NAME:        Receiver_Init
 * ACTION:      Initialise the receiver subsystem.
 */
 
void Receiver_Init( void )
{
    int Count;
    
	/* set up the initial positions for each channel */
	for (Count = 0; Count < NUM_RECEIVER_INPUTS; Count++)
	{
	    ReceiverCompletedStartPos[Count].MSBTime = 0;
	    ReceiverCompletedStartPos[Count].LSBTime = 0;
	    ReceiverCompletedEndPos[Count].MSBTime = 0;
	    ReceiverCompletedEndPos[Count].LSBTime = 0;
	}
	
    /* enable input interrupts */
    AT91F_PIO_InterruptEnable(AT91C_BASE_PIOA,
                                RECEIVER1_INPUT |
                                RECEIVER2_INPUT |
                                RECEIVER3_INPUT |
                                RECEIVER4_INPUT |
                                RECEIVER5_INPUT);
            
    AT91F_AIC_ConfigureIt(AT91C_BASE_AIC, AT91C_ID_PIOA, 6, AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL, &Receiver_IsrHandler);
    AT91F_AIC_EnableIt(AT91C_BASE_AIC, AT91C_ID_PIOA);
}


/*
 * NAME:        Receiver_Get
 * ACTION:      Get the current position of a R/C receiver input
 * PARAMETERS:  int Port - which receiver input to get (0-6)
 * RETURNS:     int - the current receiver input position. -1 if there's no signal.
 */
 
int Receiver_Get(int Port)
{
    unsigned long long CurrentTime = Timer_GetMicroSeconds();
    
    if (Port >= 0 && Port < NUM_RECEIVER_INPUTS)
    {
        unsigned long long StartTime = Timer_CookTime(ReceiverCompletedStartPos[Port]);
        unsigned long long EndTime = Timer_CookTime(ReceiverCompletedEndPos[Port]);
        unsigned long Period = EndTime - StartTime;
        
        if ( CurrentTime - EndTime > RECEIVER_TIMEOUT ||
             Period < SERVO_MIN_US ||
             Period > SERVO_MAX_US )
            return -1;
        
        return Period - SERVO_MIN_US;
    }
    else
        return 0;
}


/*
 * NAME:        Receiver_IsrHandler
 * ACTION:      Interrupt service routine for receiver inputs.
 */
 
void Receiver_IsrHandler(void)
{
    unsigned int InputChanged;
    unsigned int InputData;
    int PinCount;
    struct RawTime CurrentTime;
    
    /* which inputs have changed? */
    InputChanged = AT91F_PIO_GetInterruptStatus(AT91C_BASE_PIOA);
    InputData = AT91F_PIO_GetInput(AT91C_BASE_PIOA);
    CurrentTime = Timer_GetRawTime();
    
    /* check each of the pins */
    for (PinCount = 0; PinCount < NUM_RECEIVER_INPUTS; PinCount++)
    {
        /* did this one change? */
        if (InputChanged & ReceiverPinMask[PinCount])
        {
            /* did it change high or low? */
            if (InputData & ReceiverPinMask[PinCount])
            {
                /* input went high - start timing this pin */
                ReceiverStartPos[PinCount] = CurrentTime;
            }
            else
            {
                /* finish timing this pin - move the start and 
                 * end timings into the "completed" area */
                ReceiverCompletedStartPos[PinCount] = ReceiverStartPos[PinCount];
                ReceiverCompletedEndPos[PinCount] = CurrentTime;
            }
        }
    }
    
    /* clear the interrupt in the AIC to complete interrupt handling */ 
    AT91C_BASE_AIC->AIC_EOICR = 0;
}
